#include "WHW_IRQN.h"
#include "IMU_Task.h"
#include "operate.h"
#include "RUI_CAN.h"
#include "refree_send.h"
#include "bsp_dwt.h"
#include "cmsis_os.h"

float t1 = 0;
float t2 = 0;
float dt = 0;

void StartRefreeTask(void const * argument)
{
    portTickType currentTimeRefree;
    currentTimeRefree = xTaskGetTickCount();

    Matrixs_Init();
    for (;;)
    {
        t1 = DWT_GetTimeline_ms();
        Operate_Task();
        Data_send();
        t2 = DWT_GetTimeline_ms();
        dt = t2 - t1;
        dt/=1;
        vTaskDelayUntil(&currentTimeRefree, 34);
    }
}

void StartIMU1Task(void const * argument)
{
    portTickType currentTimeIMU1;
    currentTimeIMU1 = xTaskGetTickCount();

    Imu_Init();
    for (;;) {
        INS_Task(&IMU_Data1);
        vTaskDelayUntil(&currentTimeIMU1, 1);
    }
}

void StartIMU2Task(void const * argument)
{
    portTickType currentTimeIMU2;
    currentTimeIMU2 = xTaskGetTickCount();

//    Imu_Init2();
    for (;;) {
//        INS_Task2(&IMU_Data2);
        vTaskDelayUntil(&currentTimeIMU2, 1);
    }
}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//王涵炜
 *	@performance:	    //定时器回调函数
 *	@parameter:		    //
 *	@time:				//24-04-27 16:55
 *	@ReadMe:			//
 ************************************************************万能分隔符**************************************************************/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{

}

/************************************************************万能分隔符**************************************************************
 * 	@author:			//小瑞
 *	@performance:	    //CAN接收函数
 *	@parameter:		    //
 *	@time:				//22-11-23 20:42
 *	@ReadMe:			//
 *  @LastUpDataTime:    //2023-04-20 02:52    bestrui
 *  @UpData：           //更新成共用体
 *  @LastUpDataTime:    //2023-05-06 20:23    bestrui
 *  @UpData：           //更新判断逻辑
 ************************************************************万能分隔符**************************************************************/

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    uint8_t rx_data[8];
    CAN_RxHeaderTypeDef  RxMessage;

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxMessage, rx_data);

    if (hcan == &hcan1)
    {
        switch (RxMessage.StdId)
        {
            case 0x201:
            {
                memcpy(Angle_ICM.Data , rx_data , 8);
                break;
            }
        }
    }

    if (hcan == &hcan2)
    {
        switch (RxMessage.StdId)
        {
            case 0x201:
            {
				memcpy(Angle_DM.Data , rx_data , 8);
                q_arm[0] = ((float)Angle_DM.DataNeaten.q0/10000);
				q_arm[1] = ((float)Angle_DM.DataNeaten.q1/10000);
				q_arm[2] = ((float)Angle_DM.DataNeaten.q2/10000);
				q_arm[3] = ((float)Angle_DM.DataNeaten.q3/10000);
                break;
            }
			
			case 0x202:
            {
				memcpy(Angle_ICM.Data , rx_data , 8);
                q_fing[0] = ((float)Angle_ICM.DataNeaten.q0/10000);
				q_fing[1] = ((float)Angle_ICM.DataNeaten.q1/10000);
				q_fing[2] = ((float)Angle_ICM.DataNeaten.q2/10000);
				q_fing[3] = ((float)Angle_ICM.DataNeaten.q3/10000);
                break;
            }
        }
    }
}


